#include "bsp/bsp.h"
#include "bsp/stm32/gpio.h"
#include "os/os.h"
#include "utils/utils.h"
#include "os/queue.h"
#include "communication/message.h"
#include "bsp/can.h"

typedef struct {
	FDCAN_TxHeaderTypeDef Header;
	u8 data[8];
}can_tx_message_t;

typedef struct {
	FDCAN_RxHeaderTypeDef Header;
	u8 data[8];
}can_rx_message_t;

#define TX_NUM 32
#define RX_NUM 32
static co_queue_t g_tx_queue;
static co_queue_t g_rx_queue;
static uint8_t _g_tx_buffer[sizeof(can_tx_message_t) * TX_NUM + 1];
static uint8_t _g_rx_buffer[sizeof(can_rx_message_t) * RX_NUM + 1];
static FDCAN_HandleTypeDef hfdcan1;
static can_frame_handler _handler = NULL;

#define EXT_FITLER_NR 1

static void MX_FDCAN1_Init(void)
{

	/* USER CODE BEGIN FDCAN1_Init 0 */

	/* USER CODE END FDCAN1_Init 0 */

	/* USER CODE BEGIN FDCAN1_Init 1 */

	/* USER CODE END FDCAN1_Init 1 */
	hfdcan1.Instance = FDCAN1;
	hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1;
	hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
	hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
	hfdcan1.Init.AutoRetransmission = DISABLE;
	hfdcan1.Init.TransmitPause = DISABLE;
	hfdcan1.Init.ProtocolException = DISABLE;

	hfdcan1.Init.NominalPrescaler = 40;
	hfdcan1.Init.NominalSyncJumpWidth = 1;
	hfdcan1.Init.NominalTimeSeg1 = 2;
	hfdcan1.Init.NominalTimeSeg2 = 14;
	hfdcan1.Init.DataPrescaler = 1;
	hfdcan1.Init.DataSyncJumpWidth = 1;
	hfdcan1.Init.DataTimeSeg1 = 1;
	hfdcan1.Init.DataTimeSeg2 = 1;
	hfdcan1.Init.StdFiltersNbr = 0;
	hfdcan1.Init.ExtFiltersNbr = EXT_FITLER_NR;
	hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
#if CAN_BITRATE == CAN_BITRATE_250K
	hfdcan1.Init.NominalPrescaler = 40;
#elif CAN_BITRATE == CAN_BITRATE_500K
	hfdcan1.Init.NominalPrescaler = 20;
#endif
	if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK)
	{
		//Error_Handler();
	}
	/* USER CODE BEGIN FDCAN1_Init 2 */

	/* USER CODE END FDCAN1_Init 2 */
}
void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* hfdcan)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
  if(hfdcan->Instance==FDCAN1)
  {
  /* USER CODE BEGIN FDCAN1_MspInit 0 */

  /* USER CODE END FDCAN1_MspInit 0 */

  /** Initializes the peripherals clocks
  */
    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_FDCAN;
    PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
    {
      //Error_Handler();
    }

    /* Peripheral clock enable */
    __HAL_RCC_FDCAN_CLK_ENABLE();

    __HAL_RCC_GPIOB_CLK_ENABLE();
    /**FDCAN1 GPIO Configuration
    PB8-BOOT0     ------> FDCAN1_RX
    PB9     ------> FDCAN1_TX
    */
    /*GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF9_FDCAN1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
	*/
	gpio_af_init(PIO(PORTB, PIN8), GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW, GPIO_AF9_FDCAN1);
	gpio_af_init(PIO(PORTB, PIN9), GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW, GPIO_AF9_FDCAN1);

  /* USER CODE BEGIN FDCAN1_MspInit 1 */

  /* USER CODE END FDCAN1_MspInit 1 */

  }
}

void can_drv_filter_init(void) {
#if EXT_FITLER_NR > 0
	FDCAN_FilterTypeDef filter;
	filter.FilterIndex = 0;
	filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
	filter.FilterID1 = MY_MESSAGE_ADDR;
	filter.FilterID2 = CAN_FILTER_DEST_MASK;
	filter.IdType = FDCAN_EXTENDED_ID;
	filter.FilterType = FDCAN_FILTER_MASK;
	HAL_FDCAN_ConfigFilter(&hfdcan1, &filter);
#endif
}

static __inline__ void can_rx_fifo(int fifo){
	can_rx_message_t message;
	u8 data[64];
	if (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, fifo) > 0) {
		if (HAL_OK == HAL_FDCAN_GetRxMessage(&hfdcan1, fifo, &message.Header, data)) {
			if (message.Header.FDFormat != FDCAN_CLASSIC_CAN || message.Header.RxFrameType != FDCAN_DATA_FRAME ||
						message.Header.IdType != FDCAN_EXTENDED_ID || message.Header.DataLength > FDCAN_DLC_BYTES_8) {
				return;
			}
			memcpy(message.data, data, 8);
			queue_put(&g_rx_queue, &message);
		}
	}
}

void can_rx_poll(void){
	can_rx_message_t message;
	if (queue_get(&g_rx_queue, &message) && (_handler != NULL)) {
		_handler(message.Header.Identifier, message.data, message.Header.DataLength);
	}
}

void can_tx_poll(void){
	can_tx_message_t can_tr_m;
	if ((__HAL_FDCAN_GET_FLAG(&hfdcan1, (FDCAN_FLAG_ERROR_PASSIVE|FDCAN_FLAG_ERROR_LOGGING_OVERFLOW|FDCAN_FLAG_BUS_OFF)))){
		can_drv_reset();
	}
	while (HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) > 0) {
		if (!queue_get(&g_tx_queue, &can_tr_m)) {
			break;
		}
		HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &can_tr_m.Header, can_tr_m.data);
	}
}

static int can_queue_tx_message(can_tx_message_t *P_message){
	can_tx_poll();
	int len = sizeof(can_tx_message_t);
	if (queue_put(&g_tx_queue, P_message)){
		return 0;
	}
	return -1;
}

int can_send_message(uint32_t can_id, const void*buff, int len) {
	can_tx_message_t trasnmit_msg;
	head_t can_frame_id;
	u32 total_frames = ((len + 7) >> 3);
	int send_len = len;
	u32 frame_id = 1;
	can_frame_id.id = can_id & 0x1FFFFFFF;
	can_frame_id.total = total_frames;

	while(send_len > 0){
		int dlen = _min(8,send_len);
		can_frame_id.idx = frame_id;
		trasnmit_msg.Header.Identifier	= can_frame_id.id;
		trasnmit_msg.Header.DataLength  = dlen;
		trasnmit_msg.Header.IdType		= FDCAN_EXTENDED_ID;
		trasnmit_msg.Header.FDFormat	= FDCAN_CLASSIC_CAN;
		trasnmit_msg.Header.TxFrameType = FDCAN_DATA_FRAME;
		trasnmit_msg.Header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
		trasnmit_msg.Header.MessageMarker      = 0;
		trasnmit_msg.Header.BitRateSwitch      = FDCAN_BRS_OFF;
		trasnmit_msg.Header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;

		memcpy((char *)trasnmit_msg.data, (char *)buff + (len - send_len), dlen);
		send_len -= dlen;
		frame_id ++;
		if (can_queue_tx_message(&trasnmit_msg) != 0){
			return -1;
		}
	}

	return 0;
}

int can_send_ext_message(uint32_t can_id, const void*buff, int len) {
	can_tx_message_t trasnmit_msg;

	trasnmit_msg.Header.Identifier	= can_id & 0x1FFFFFFF;
	trasnmit_msg.Header.DataLength  = len;
	trasnmit_msg.Header.IdType		= FDCAN_EXTENDED_ID;
	trasnmit_msg.Header.FDFormat	= FDCAN_CLASSIC_CAN;
	trasnmit_msg.Header.TxFrameType = FDCAN_DATA_FRAME;
	trasnmit_msg.Header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
	trasnmit_msg.Header.MessageMarker      = 0;
	trasnmit_msg.Header.BitRateSwitch      = FDCAN_BRS_OFF;
	trasnmit_msg.Header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;

	memcpy((char *)trasnmit_msg.data, (char *)buff, len);
	if (can_queue_tx_message(&trasnmit_msg) != 0){
		return -1;
	}

	return 0;

}


static void can_hw_init(void) {
	MX_FDCAN1_Init();
	can_drv_filter_init();
	HAL_FDCAN_ConfigInterruptLines(&hfdcan1, FDCAN_IT_GROUP_RX_FIFO0|FDCAN_IT_GROUP_RX_FIFO1, FDCAN_INTERRUPT_LINE0);
	HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_LIST_RX_FIFO0|FDCAN_IT_LIST_RX_FIFO1, 0);
    HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, CAN_IRQ_PRIORITY, 0);
    HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn);
	HAL_FDCAN_Start(&hfdcan1);
}

void can_drv_reset(void){
	can_drv_deinit();
	can_hw_init();
}

void can_drv_deinit(void){
	HAL_FDCAN_Stop(&hfdcan1);
	HAL_FDCAN_DeInit(&hfdcan1);
}

static u32 _can_poll_task(void *args) {
	can_rx_poll();
	can_tx_poll();
	return 0;
}

void can_drv_init(can_frame_handler handler) {
	_handler = handler;
	g_tx_queue = queue_create_with_buffer(_g_tx_buffer, sizeof(_g_tx_buffer), sizeof(can_tx_message_t));
	g_rx_queue = queue_create_with_buffer(_g_rx_buffer, sizeof(_g_rx_buffer), sizeof(can_rx_message_t));
	os_task_create(_can_poll_task, NULL);
	can_hw_init();
}

#define FDCAN_RX_FIFO0_MASK (FDCAN_IR_RF0L | FDCAN_IR_RF0F | FDCAN_IR_RF0N)
#define FDCAN_RX_FIFO1_MASK (FDCAN_IR_RF1L | FDCAN_IR_RF1F | FDCAN_IR_RF1N)

void FDCAN1_IT0_IRQHandler(void) {
	u32 RxFifo0ITs = hfdcan1.Instance->IR & FDCAN_RX_FIFO0_MASK;
  	RxFifo0ITs &= hfdcan1.Instance->IE;

	u32 RxFifo1ITs = hfdcan1.Instance->IR & FDCAN_RX_FIFO1_MASK;
	RxFifo1ITs &= hfdcan1.Instance->IE;

	if (RxFifo0ITs != 0U)
	{
		/* Clear the Rx FIFO 0 flags */
		__HAL_FDCAN_CLEAR_FLAG(&hfdcan1, RxFifo0ITs);
		can_rx_fifo(FDCAN_RX_FIFO0);
	}
	
	if (RxFifo1ITs != 0U)
	{
		/* Clear the Rx FIFO 1 flags */
		__HAL_FDCAN_CLEAR_FLAG(&hfdcan1, RxFifo1ITs);
		can_rx_fifo(FDCAN_RX_FIFO1);
	}
}

